



static void InvertUint16(unsigned short *dBuf,unsigned short *srcBuf)
{
    int i;
    unsigned short tmp[4]={0};

    for(i=0;i< 16;i++)
    {
        if(srcBuf[0]& (1 << i))
        tmp[0]|=1<<(15 - i);
    }
    dBuf[0] = tmp[0];
}

//unsigned short CRC16_MODBUS(unsigned char *data, unsigned int datalen)
unsigned short CRC16(unsigned char *data, unsigned int datalen)
{
    int i;
    unsigned short wCRCin = 0xFFFF;
    unsigned short wCPoly = 0x8005;

    InvertUint16(&wCPoly,&wCPoly);
    while (datalen--)
    {
        wCRCin ^= *(data++);
        for(i = 0;i < 8;i++)
        {
            if(wCRCin & 0x01)
                wCRCin = (wCRCin >> 1) ^ wCPoly;
            else
                wCRCin = wCRCin >> 1;
        }
    }
    return (wCRCin);
}

